
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ahrs.c
  * @author     baiyang
  * @date       2021-10-31
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <rtthread.h>
#include <rtdevice.h>

#include "define.h"

#include "ahrs.h"
#include <ahrs/ahrs_ekf.h>
#include <ahrs/ahrs_sensor.h>
#include <common/microbee.h>
#include <perf/perf_counter.h>
#include <common/time/gp_time.h>
#include <board_config/board_config.h>
#include <common/console/console.h>
/*-----------------------------------macro------------------------------------*/
#define EVENT_AHRS_FAST_LOOP        (1<<0)
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void ahrs_nav_init();
/*----------------------------------variable----------------------------------*/
#if defined(HAL_NO_CCMRAM)
static char thread_ahrs_ekf_stack[1024*12];
#else
static char thread_ahrs_ekf_stack[1024*12] RT_SECTION(".ccmram");
#endif

struct rt_thread thread_ahrs_ekf_handle;

perf_counter_t _ahrs_elapsed_perf;
perf_counter_t _ahrs_interval_perf;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static void ahrs_nav_init()
{
    ahrs_sensor_init(1000/PERIOD_MS_250HZ);
    ahrs_ekf_init();
    ahrs_gcs_init();
}

void ahrs_nav_run(void *parameter)
{
    rt_thread_mdelay(10);

    while(1) {
        perf_begin(_ahrs_elapsed_perf);
        perf_count(_ahrs_interval_perf);

        ahrs_sensor_update();

        uint32_t now_ms = time_millis();

        TIMETAG_CHECK_EXECUTE2(ahrs_gcs, PERIOD_MS_100HZ, now_ms, ahrs_gcs_update(););
        TIMETAG_CHECK_EXECUTE2(ahrs_ekf, PERIOD_MS_250HZ, now_ms, ahrs_ekf_update(););

        perf_end(_ahrs_elapsed_perf);
        rt_thread_mdelay(1);
    }
}

void task_ahrs_nav_init()
{
    rt_err_t res;

    // 这个函数不能移动到navigation_entry里
    // ahrs_nav_init中有传感器初始化函数，有
    // 可能会使用spi读写，DMA不能访问ccmram内存
    ahrs_nav_init();

    _ahrs_elapsed_perf  = perf_alloc(PC_ELAPSED,   "ahrs: ET");
    _ahrs_interval_perf = perf_alloc(PC_INTERVAL,  "ahrs: INR");

    res = rt_thread_init(&thread_ahrs_ekf_handle,
                           "ahrs_ekf",
                           ahrs_nav_run,
                           RT_NULL,
                           &thread_ahrs_ekf_stack[0],
                           sizeof(thread_ahrs_ekf_stack),PRIORITY_AHRS,5);
    if (res == RT_EOK)
        rt_thread_startup(&thread_ahrs_ekf_handle);

    brd_set_vehicle_init_stage(INIT_STAGE_AHRS_NAV);
}

/*------------------------------------test------------------------------------*/


